#include "gp_qpcr_ctl.h"

#include "sock_msg_ctl.h"
#include "gp_layout_map.h"
#include "stdio.h"
#include "string.h"
#include "../lua/action_msg.h"
#include "resource_manage.h"
#include <unistd.h>
#include "gp_subboard_ctl.h"
#include "../data_proc/board_data_proc.h"
#include "json/json.h"
#include <iostream>
#include <fstream>
#include <string>


void qpcrActFunc(void * paramIn,void * actResultOut)
{
    if(!paramIn || !actResultOut)
    {
        return;
    }
    printf("enter in qpcrActFunc\n");
    act_msg_node * pstActMsg = (act_msg_node *)paramIn;
    int actSuccess = 0;
    
    if(pstActMsg->msgHead.actType == ACT_PARRAL_MOVE)
    {
        if(pstActMsg->msgHead.paramType == PARAM_AXIS_MOVE_POS)
        {
            char * pData = (char*)(pstActMsg->paramData);
            location_layout locLayout;

            int qpcrId = 0;

            memcpy(&locLayout,pData+sizeof(int),sizeof(location_layout));

            if(pstActMsg->msgHead.moduleType == MODULE_QPCR1)
            {
                qpcrId = 0; 
            }
            else if(pstActMsg->msgHead.moduleType == MODULE_QPCR2)
            {
                qpcrId = 1;
            }
            else
            {
                qpcrId = -1;
                actSuccess = 0;
                printf("in ACT_PARRAL_MOVE, module type invalid,moudle:%d\n",pstActMsg->msgHead.moduleType);
            }
            
            printf("in ACT_PARRAL_MOVE, location_type:%d\n",locLayout.location_type);
            
            if(qpcrId==0 || qpcrId == 1)
            {
                if(locLayout.location_type == QPCR1_BATH_X_WAIT 
                || locLayout.location_type == QPCR1_BATH_Z_WAIT)
                {
                   actSuccess = qpcr_pull_out(qpcrId); 
                }
                else if(locLayout.location_type == QPCR1_BATH_X_WORK 
                || locLayout.location_type == QPCR1_BATH_Z_WORK)
                {
                   actSuccess = qpcr_push_in(qpcrId);                     
                }
            }
        }
    }
    else if(pstActMsg->msgHead.actType == ACT_SINGLE_MOVE)
    {
        if(pstActMsg->msgHead.paramType == PARAM_AXIS_MOVE_STEPS)
        {
            int component = -1;
            int steps = 0;
            char * pData = (char*)pstActMsg->paramData;
           
            memcpy(&component,pData,sizeof(int));
            pData += sizeof(int);

            memcpy(&steps,pData,sizeof(int));

            int indexQpcr = 0;
            
            if(pstActMsg->msgHead.moduleType == MODULE_QPCR1)
            {
                indexQpcr = 0; 
            }
            else if(pstActMsg->msgHead.moduleType == MODULE_QPCR2)
            {
                indexQpcr = 1;
            }
            else
            {
                indexQpcr = -1;
            }

            printf("qpcrActFunc,single move steps:%d,componet:%d\n",steps,component);

            if(indexQpcr < 0)
            {
                actSuccess = 0;
            }
            else
            {               
                actSuccess = qpcr_single_move_steps(&steps,component,indexQpcr);
            }            
        }
        else if(pstActMsg->msgHead.paramType == PARAM_AXIS_MOVE_POS)
        {
            printf("single move pos \n");
            char * pData = (char*)(pstActMsg->paramData);
            location_layout locLayout;
            int axixType = 0;
            memcpy(&axixType,pData,sizeof(int));
            pData += sizeof(int);
            memcpy(&locLayout,pData,sizeof(location_layout));
            pData += sizeof(location_layout);



            int qpcrId = 0;

            if(pstActMsg->msgHead.moduleType == MODULE_QPCR1)
            {
                qpcrId = 0;
            }
            else
            {
                qpcrId = 1;
            }

            printf("axixType:%d\n",axixType);

            if(axixType == MT_OPTICAL)
            {
                printf("optical move\n");
                int chanId = locLayout.location_type;

                if(0 <= chanId && chanId < OPTICS_CHANNEL_MAX)
                {                    
                    actSuccess = qpcr_optical_pos(chanId,qpcrId);
                }
            }
            else
            {
                actSuccess = bath_move_pos(qpcrId,axixType,&locLayout);
            }

        }
        
    }
    else if(pstActMsg->msgHead.actType == ACT_SCHEME_CTL
    || pstActMsg->msgHead.actType == ACT_LIGHT_CTL
    || pstActMsg->msgHead.actType == ACT_HEAT_CTL
    || pstActMsg->msgHead.actType == ACT_TEMPERATURE_READ)
    {
        subboardActFunc(pstActMsg,actResultOut);
        return;
    }
    else if(pstActMsg->msgHead.actType == ACT_SINGLE_INIT)
    {
        int qpcrId = 0;

        if(pstActMsg->msgHead.moduleType == MODULE_QPCR1)
        {
            qpcrId = 0;
        }
        else
        {
            qpcrId = 1;
        }

        int component = -1;
        memcpy(&component,(char*)pstActMsg->paramData,sizeof(int));
        printf("qpcrActFunc,single init,componet:%d\n",component);
        
        actSuccess = qpcr_single_init(qpcrId, component);
    }
    else if(pstActMsg->msgHead.actType == ACT_MODULE_INIT)
    {
        int qpcrId = 0;

        if(pstActMsg->msgHead.moduleType == MODULE_QPCR1)
        {
            qpcrId = 0;
        }
        else
        {
            qpcrId = 1;
        }
        actSuccess = qpcr_module_init(qpcrId);
    }

    msgAckSock * pstResult = (msgAckSock *)actResultOut;

    if(!actSuccess)
    {
        pstResult->status = 1;
    }
    else
    {
        pstResult->status = 0;
    }
}

int qpcr_single_init(int qpcrId, int axisTypeIn)
{
    int actSuccess = 0;
    
    move_param moveParam[2];

    memset(&moveParam,0,sizeof(move_param));

    if(axisTypeIn == MT_AXIS_Z)
    {
        return qpcr_z_init(qpcrId);
    }
    
    
    int sid = convertToQpcrSid(axisTypeIn,qpcrId);
        
    if(sid==-1)
    {
        return actSuccess;
    }
    
    moveParam[0].sid = sid;
    moveParam[0].pos = 0;

    moveParam[0].dec_speed = 80000;
    moveParam[0].inc_speed = 80000;
    moveParam[0].speed = 20000;
    
    if(axisTypeIn == MT_OPTICAL)
    {
        actSuccess = uimMoveAct_multi(moveParam, 1, 20000);
    }
    else
    {
        actSuccess = uimInitAct_multi(moveParam, 1, 20000);
    }   
    

    return actSuccess;
}

int convertToQpcrSid(int componentIn, int numQpcr)
{
    int sid = -1;
    switch (componentIn)
    {
        case MT_AXIS_Y:
        {
            if(numQpcr == 0)
            {
               sid = SID_QPCR1_Y;
            }
            else
            {
               sid = SID_QPCR2_Y;
            }

        }
        break;
        case MT_OPTICAL:
        {
            if(numQpcr == 0)
            {
               sid = SID_QPCR1_OPTICAL;
            }
            else
            {
               sid = SID_QPCR2_OPTICAL;
            }
        }
        default:
        break;
    }
    return sid; 
}

int qpcr_single_move_steps(void * paramIn,int component,int indexQpcr)
{
    move_param moveParam[2];
    int numAct = 1;
    int * pSteps = (int *)paramIn;
    int actSuccess = 0;
    if(component == MT_AXIS_Z)
    {
        numAct = 2;
         
        moveParam[0].sid = indexQpcr?SID_QPCR2_ZL:SID_QPCR1_ZL;
        moveParam[0].pos = *pSteps;

        moveParam[0].dec_speed = 8000;
        moveParam[0].inc_speed = 8000;
        moveParam[0].speed = 8000;

        moveParam[1].sid = indexQpcr?SID_QPCR2_ZR:SID_QPCR1_ZR;
        moveParam[1].pos = *pSteps;

        moveParam[1].dec_speed = 8000;
        moveParam[1].inc_speed = 8000;
        moveParam[1].speed = 8000;
    }
    else
    {
        int sid = convertToQpcrSid(component,indexQpcr);
        
        if(sid==-1)
        {
            return actSuccess;
        }

        moveParam[0].sid = sid;
        moveParam[0].pos = *pSteps;

        moveParam[0].dec_speed = 8000;
        moveParam[0].inc_speed = 8000;
        moveParam[0].speed = 8000;
    }
    
    actSuccess = uimMoveAct_multi(moveParam, numAct, 20000);

    return actSuccess;
}

int qpcr_optical_pos(int chanId,int qpcrId)
{
    move_param moveParam;
    int actSuccess = 0;

    //int stepsChan[5] = {9000,18000,27000,37000,47000};
    CGpLayout * pstlayoutMng = CGpLayout::get_instance();

    int channelPos = pstlayoutMng->getQpcrOpticalPos(qpcrId,chanId);

    printf("qpcrId:%d,chanId:%d,channelPos:%d\n",qpcrId,chanId,channelPos);
    if(channelPos < 0)
    {        
        return actSuccess;
    }
    
    moveParam.sid = qpcrId?SID_QPCR2_OPTICAL:SID_QPCR1_OPTICAL;
    moveParam.pos = channelPos;//stepsChan[chanId];

    moveParam.dec_speed = 160000;
    moveParam.inc_speed = 160000;
    moveParam.speed = 32000;

    printf("qpcr optical sid:%d,pos:%d\n",moveParam.sid,moveParam.pos);
    
    actSuccess = uimMoveAct_multi(&moveParam, 1, 20000);

    return actSuccess;
}

int qpcr_z_init(int qpcrId)
{
    move_param moveParam[2];
    int numAct = 1;
    int actSuccess = 0;

    numAct = 2;
        
    moveParam[0].sid = qpcrId?SID_QPCR2_ZL:SID_QPCR1_ZL;
    moveParam[0].pos = 0;

    moveParam[0].dec_speed = 80000;
    moveParam[0].inc_speed = 80000;
    moveParam[0].speed = 20000;

    moveParam[1].sid = qpcrId?SID_QPCR2_ZR:SID_QPCR1_ZR;
    moveParam[1].pos = 0;

    moveParam[1].dec_speed = 80000;
    moveParam[1].inc_speed = 80000;
    moveParam[1].speed = 20000;

    actSuccess = uimInitAct_multi(moveParam, 2, 20000);

    return actSuccess;
}

int qpcr_module_init(int qpcrId)
{

    int actSuccess = 0;
    int sidzl = SID_QPCR1_ZL;
    int sidzr = SID_QPCR1_ZR;

    int sidy = SID_QPCR1_Y;


    if(qpcrId)
    {
        sidzl = SID_QPCR2_ZL;
        sidzr = SID_QPCR2_ZR;

        sidy = SID_QPCR2_Y;        
    }


    move_param moveParamArr[2];

    memset(moveParamArr,0,sizeof(move_param)*2);

    moveParamArr[0].sid = sidzl;
    moveParamArr[0].speed = 8000;
    moveParamArr[0].inc_speed = 8000; 
    moveParamArr[0].dec_speed = 8000;
    moveParamArr[0].pos = 0; 

    moveParamArr[1].sid = sidzr;
    moveParamArr[1].speed = 8000;
    moveParamArr[1].inc_speed = 8000; 
    moveParamArr[1].dec_speed = 8000;
    moveParamArr[1].pos = 0;

    actSuccess = uimInitAct_multi(moveParamArr,2,20000);

    if(actSuccess!=1)
    {
        printf("in qpcr_module_init, z go zero failed\n");
        return 0;
    }


    moveParamArr[0].sid = sidy;
    moveParamArr[0].speed = 4000;
    moveParamArr[0].inc_speed = 8000; 
    moveParamArr[0].dec_speed = 8000;
    moveParamArr[0].pos = 0;

    actSuccess = uimInitAct_multi(moveParamArr,1,10000);

    if(actSuccess!= 1)
    {
        printf("in qpcr_module_init, y go zero failed\n");
    }

    return actSuccess; 
}

int qpcr_push_in(int qpcrIdIn)
{
    int actSuccess = 0;
    CGpLayout * pstlayoutMng = CGpLayout::get_instance();

    if(!pstlayoutMng)
    {
        return actSuccess;
    }

    locs_qpcr * pstloc = pstlayoutMng->getQpcrPos(qpcrIdIn);

    if(!pstloc)
    {
        printf("in qpcr_push_in, get qpcr locs failed\n");
        return actSuccess;
    }

    int sidzl = SID_QPCR1_ZL;
    int sidzr = SID_QPCR1_ZR;

    int sidy = SID_QPCR1_Y;


    if(qpcrIdIn)
    {
        sidzl = SID_QPCR2_ZL;
        sidzr = SID_QPCR2_ZR;

        sidy = SID_QPCR2_Y;        
    }

    move_param moveParamArr[2];
    memset(moveParamArr,0,sizeof(move_param)*2);

    moveParamArr[0].sid = sidzl;
    moveParamArr[0].speed = 8000;
    moveParamArr[0].inc_speed = 8000; 
    moveParamArr[0].dec_speed = 8000;
    moveParamArr[0].pos = 0; 

    moveParamArr[1].sid = sidzr;
    moveParamArr[1].speed = 8000;
    moveParamArr[1].inc_speed = 8000; 
    moveParamArr[1].dec_speed = 8000;
    moveParamArr[1].pos = 0;

    actSuccess = uimMoveAct_multi(moveParamArr,2,20000);

    if(actSuccess!=1)
    {
        printf("in qpcr_push_in, z go zero failed\n");
        return 0;
    }

    moveParamArr[0].sid = sidy;
    moveParamArr[0].speed = 8000;
    moveParamArr[0].inc_speed = 8000; 
    moveParamArr[0].dec_speed = 8000;
    moveParamArr[0].pos = pstloc->bath_x_work;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("in qpcr_push_in, y go to work pos failed\n");
        return 0;
    }

    moveParamArr[0].sid = sidzl;
    moveParamArr[0].speed = 8000;
    moveParamArr[0].inc_speed = 8000; 
    moveParamArr[0].dec_speed = 8000;
    moveParamArr[0].pos = pstloc->bath_z_work; 

    moveParamArr[1].sid = sidzr;
    moveParamArr[1].speed = 8000;
    moveParamArr[1].inc_speed = 8000; 
    moveParamArr[1].dec_speed = 8000;
    moveParamArr[1].pos = pstloc->bath_z_work;

    actSuccess = uimMoveAct_multi(moveParamArr,2,20000);

    if(actSuccess!=1)
    {
        printf("in qpcr_push_in, z go up failed\n");
    }

    return actSuccess;   

}

int qpcr_pull_out(int qpcrIdIn)
{
    int actSuccess = 0;
    CGpLayout * pstlayoutMng = CGpLayout::get_instance();

    if(!pstlayoutMng)
    {
        return actSuccess;
    }

    locs_qpcr * pstloc = pstlayoutMng->getQpcrPos(qpcrIdIn);

    if(!pstloc)
    {
        printf("in qpcr_pull_out, get qpcr locs failed,qpcrIdIn:%d\n",qpcrIdIn);
        return actSuccess;
    }

    int sidzl = SID_QPCR1_ZL;
    int sidzr = SID_QPCR1_ZR;

    int sidy = SID_QPCR1_Y;


    if(qpcrIdIn)
    {
        sidzl = SID_QPCR2_ZL;
        sidzr = SID_QPCR2_ZR;

        sidy = SID_QPCR2_Y;        
    }

    move_param moveParamArr[2];
    memset(moveParamArr,0,sizeof(move_param)*2);

    moveParamArr[0].sid = sidzl;
    moveParamArr[0].speed = 8000;
    moveParamArr[0].inc_speed = 8000; 
    moveParamArr[0].dec_speed = 8000;
    moveParamArr[0].pos = 0; 

    moveParamArr[1].sid = sidzr;
    moveParamArr[1].speed = 8000;
    moveParamArr[1].inc_speed = 8000; 
    moveParamArr[1].dec_speed = 8000;
    moveParamArr[1].pos = 0;

    actSuccess = uimMoveAct_multi(moveParamArr,2,20000);

    if(actSuccess!=1)
    {
        printf("in qpcr_pull_out, z go zero failed\n");
        return 0;
    }

    moveParamArr[0].sid = sidy;
    moveParamArr[0].speed = 8000;
    moveParamArr[0].inc_speed = 8000; 
    moveParamArr[0].dec_speed = 8000;
    moveParamArr[0].pos = pstloc->bath_x_wait;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("in qpcr_push_in, y go to work pos failed\n");
    }

    return actSuccess;  
}


int bath_move_pos(int qpcrId, int axisTypeIn, void * locIn)
{
   int actSuccess = 0;
   location_layout locLayout;

   memcpy(&locLayout,locIn,sizeof(location_layout));


   CGpLayout * layoutHandle = CGpLayout::get_instance();


   locs_qpcr * pstLocQpcr = layoutHandle->getQpcrPos(qpcrId);
   int targetPos = -1;

   switch(locLayout.location_type)
   {
       case QPCR1_BATH_X_WAIT:
       case QPCR2_BATH_X_WAIT:
       {
           targetPos = pstLocQpcr->bath_x_wait;
       }
       break;
       case QPCR1_BATH_X_WORK:
       case QPCR2_BATH_X_WORK:
       {
           targetPos = pstLocQpcr->bath_x_work;
       }
       break;
       case QPCR1_BATH_Z_WORK:
       case QPCR2_BATH_Z_WORK:
       {
           targetPos = pstLocQpcr->bath_z_work;
       }
       break;
       case QPCR1_BATH_Z_WAIT:
       case QPCR2_BATH_Z_WAIT:
       {
           targetPos = pstLocQpcr->bath_z_wait;
       }
       break;

       default:
       break;      
   }

   if(targetPos<0)
   {
       printf("bath_move_pos, pos type is not valid\n");
       return actSuccess;
   }

   if(axisTypeIn != MT_AXIS_Z)
   {
       //if it is to wait pos, go down firstly
       //if it is to work pos, it is assumed that z is on 0, or it is working, no need z go down
       if(locLayout.location_type == QPCR1_BATH_X_WAIT
       ||locLayout.location_type == QPCR2_BATH_X_WAIT)
       {
            int tempPos = 0;
            actSuccess = qpcr_single_move_steps(&tempPos,MT_AXIS_Z,qpcrId);

            if(actSuccess!=1)
            {
                printf("qpcr z go down failed\n");
                return 0;
            }
       }

   }

   return qpcr_single_move_steps(&targetPos,axisTypeIn,qpcrId);
}